#include "TSDA.h"
#include "BSP.h"

enum FunctionCode
{
    SEND_CODE = 0x1A,
    RECEIVE_CODE = 0x1B,
    ERROR_CODE = 0x1C,
    READ_CODE = 0x2A
};

#ifndef PI
#define PI 3.1415926535f
#endif

// 启动电机
void TSDA_Start(void)
{
    uint8_t cmd[8] = {0};

    cmd[0] = TSDA_INDEX;
    cmd[1] = SEND_CODE;
    cmd[2] = 0x00;
    cmd[3] = 0x00;
    cmd[4] = 0x01;
    cmd[5] = 0xff;
    cmd[6] = 0x00;
    cmd[7] = 0x00;

    while (HAL_CAN_GetTxMailboxesFreeLevel(&TSDA_CAN) == 0)
    {
        continue;
    }
    CAN_Transmit(&TSDA_CAN, TSDA_ID, cmd);
}

// 关闭电机
void TSDA_Close(void)
{
    uint8_t cmd[8] = {0};

    cmd[0] = TSDA_INDEX;
    cmd[1] = SEND_CODE;
    cmd[2] = 0x00;
    cmd[3] = 0x00;
    cmd[4] = 0x00;
    cmd[5] = 0xff;
    cmd[6] = 0x00;
    cmd[7] = 0x00;

    while (HAL_CAN_GetTxMailboxesFreeLevel(&TSDA_CAN) == 0)
    {
        continue;
    }
    CAN_Transmit(&TSDA_CAN, TSDA_ID, cmd);
}

/**
 * 设置电机速度
 * 单位rad/s
 */
void TSDA_SetSpeed(float speed)
{
    int16_t v = (int16_t)(speed / 8192.0f * 100.0f * PI);

    uint8_t cmd[8] = {0};

    cmd[0] = TSDA_INDEX;
    cmd[1] = SEND_CODE;
    cmd[2] = 0x0a;
    cmd[3] = 1; // 0到3000RPM的加速时间是100ms
    cmd[4] = 1;
    cmd[5] = 0x06;
    cmd[6] = v >> 8;
    cmd[7] = v;

    while (HAL_CAN_GetTxMailboxesFreeLevel(&TSDA_CAN) == 0)
    {
        continue;
    }
    CAN_Transmit(&TSDA_CAN, TSDA_ID, cmd);
}
